/*
 *  sendOutput.cpp
 *  Ball and Plate
 *
 *  Created by Kenn Sebesta on 2008-01-23.
 *  Copyright 2007 __MyCompanyName__. All rights reserved.
 *
 */

#include "sendOutput.h"
#include <iostream>

void writeToPort( int, TranslatedOutput );
void translateOutput( CvMat *, TranslatedOutput * );

extern TableCalibration calLUT[3];
extern CvMat *levelCalibration;
//SerialStream serialPort( COM_PORT );

void sendOutput(int fd, CvMat *u) {
    TranslatedOutput u_T; //Memory leak?
    translateOutput( u, &u_T ); //Maps a control to realworld inputs
    writeToPort( fd, u_T );
}

void translateOutput(CvMat *u, TranslatedOutput *output) {
    // Any final safety checks should go here.
    
    
    // Look Up Table and interpolation. Converges quadratically and takes n+2 iterations, where 2^n > length.of.LUT
    // The LUT *must* be monotonic, i.e. entirely increasing or entirely decreasing.
    float targetVal, middleVal, lowerVal, upperVal;
    unsigned int lowerIndex, upperIndex, end;
    
    for (int i=0;i < u->rows; i++){
        end=calLUT[i].lut->rows;
        lowerIndex=0;
        upperIndex=end-1;
        targetVal=cvmGet( u, i, 0 )*10e04+cvmGet( levelCalibration, i, 0 );
        do{
            lowerVal =  cvmGet( calLUT[i].lut, lowerIndex, 1 );
            upperVal =  cvmGet( calLUT[i].lut, upperIndex, 1 );
            middleVal = cvmGet( calLUT[i].lut, (lowerIndex+upperIndex)/2, 1 );
            if (targetVal > middleVal)
                lowerIndex=(unsigned int)floor((float)(lowerIndex+upperIndex)/2);
            else
                upperIndex=(unsigned int)ceil((float)(lowerIndex+upperIndex)/2);
        }while(!((lowerVal == cvmGet(calLUT[i].lut, lowerIndex, 1)) && (upperVal == cvmGet(calLUT[i].lut, upperIndex, 1))));
        cvmGet(calLUT[i].lut, upperIndex, 1);
        cvmGet(calLUT[i].lut, lowerIndex, 1);
        output->fl[i]=cvRound(cvmGet(calLUT[i].lut, lowerIndex, 0)+(cvmGet(calLUT[i].lut, upperIndex, 0)-cvmGet(calLUT[i].lut, lowerIndex, 0))/(upperVal-lowerVal)*(targetVal-lowerVal));
    }
    return;
}

void writeToPort( int fd, TranslatedOutput output ){    
    #ifdef SERIAL
    char szBuf[30];
    sprintf (szBuf, "#0 P%d #1 P%d S1000 T210\r", (int)output.fl[0], (int)output.fl[1]);
    
    int n = write(fd, szBuf, 29);
    if (n < 0)
        fputs("write() of 29 bytes failed!\n", stderr);
    #endif
    
    return;    
}


int openPort( ) {
    int fd=-1; /* File descriptor for the port */
    #ifdef SERIAL
    
    fd = open(COM_PORT, O_RDWR | O_NOCTTY | O_NDELAY);
    if (fd == -1) {
        // If program cannot open the port.
        perror("open_port: Unable to open COM_PORT - ");
    }
    else
        fcntl(fd, F_SETFL, FNDELAY);   
    struct termios options;
    
     // Get the current options for the port...
    tcgetattr(fd, &options);
    
     // Set the baud rates...
    cfsetispeed(&options, BAUD_RATE);
    cfsetospeed(&options, BAUD_RATE);
    
    //Enable the receiver and set local mode...
    options.c_cflag |= (CLOCAL | CREAD);
    
     // Set the new options for the port...
    tcsetattr(fd, TCSANOW, &options);
    options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
    options.c_oflag &= ~OPOST;
    
    #endif
    return fd;
}

void closePort( int fd ) {
    #ifdef SERIAL
    close(fd);
    
    #endif
    return;
}
